#include <iostream>
#include <cmath>
#include <memory>
#include <string>
#include <opencv2/opencv.hpp>

cv::Mat img;

cv::Point2f cur_pt{-1, -1};             // 现坐标
cv::Point2f pid_pt{-1, -1};             // PID坐标
cv::Point2f force(0, 0);                // 控制力
cv::Point2f friction(0, 0);             // 摩擦力
cv::Point2f total_force(0, 0);          // 合力
cv::Point2f acceleration(0, 0);         // 加速度
cv::Point2f velocity(0, 0);             // 速度
cv::Point2f err(0, 0);                  // 当前误差
cv::Point2f last_err(0, 0);             // 上次误差
cv::Point2f integration_sum(0, 0);      // 累计误差
cv::Point2f derivative(0, 0);           // 微分误差
double m = 1;                           // 质量
double u = 0.9;                         // 摩擦系数
double m_dt = 0.05;                      // delta_t
double integration_thr = 200;           // 积分误差限

int kp = 0.0 , ki = 0.0  , kd = 0.0;
double Kp ,Ki ,Kd;

cv::Point2f pidResolve(cv::Point2f t_err, double kp, double ki, double kd)
{
    err = t_err;
    integration_sum += err;
    if(abs(integration_sum.x) > integration_thr)
    {
        integration_sum.x = integration_thr * abs(integration_sum.x) / integration_sum.x;
    }
    if(abs(integration_sum.y) > integration_thr)
    {
        integration_sum.y = integration_thr * abs(integration_sum.y) / integration_sum.y;
    }
    derivative = err - last_err;
    last_err = err;
    cv::Point2f t_force = kp * err + ki * integration_sum + kd * derivative;
    return t_force;
}

// event鼠标事件代号，x,y鼠标坐标，flags拖拽和键盘操作的代号
void mouseEvent(int event, int x, int y, int flags, void *ustc)
{
	if (event == CV_EVENT_MOUSEMOVE)
	{
		cur_pt = cv::Point(x, y);
	}
}

void on_trackbar_p(int ,void*){Kp=kp/100.0;}
void on_trackbar_i(int ,void*){Ki=ki/100.0;}
void on_trackbar_d(int ,void*){Kd=kd/50.0;}


int main()
{
    cv::namedWindow("img");
    cv::setMouseCallback("img", mouseEvent);       // 调用回调函数

    cv::createTrackbar( "Kp", "img", &kp, 1000, on_trackbar_p );
    cv::createTrackbar( "Ki", "img", &ki, 1000, on_trackbar_i );
    cv::createTrackbar( "Kd", "img", &kd, 1000, on_trackbar_d );
    on_trackbar_p(0,0);
    on_trackbar_i(0,0);
    on_trackbar_d(0,0);

    
    while (true)
    {
        img = cv::Mat(1024, 1280, CV_8UC3, cv::Scalar(255, 255, 255));
       
        std::string P = std::to_string(Kp);
        std::string I = std::to_string(Ki);
        std::string D = std::to_string(Kd);
        cv::putText(img,"Kp="+P, cv::Point(300,500),cv::FONT_HERSHEY_PLAIN,2, cv::Scalar(0,0,255),1, 8, false );
        cv::putText(img,"Ki="+I, cv::Point(300,400),cv::FONT_HERSHEY_PLAIN,2, cv::Scalar(0,0,255),1, 8, false );
        cv::putText(img,"Kd="+D, cv::Point(300,300),cv::FONT_HERSHEY_PLAIN,2, cv::Scalar(0,0,255),1, 8, false );

        // pid解算
        force = pidResolve((cur_pt - pid_pt), Kp, Ki, Kd);

        // 运动状态计算
        friction = -u * velocity;
        total_force = force + friction;
        acceleration = total_force / m;
        velocity += acceleration * m_dt;
        pid_pt += velocity * m_dt;

        // 状态量，必要时可解注释
        // std::cout << "force: " << force << std::endl;
        // std::cout << "friction: " << friction << std::endl;
        // std::cout << "total_force: " << total_force << std::endl;
        // std::cout << "acceleration: " << acceleration << std::endl;
        // std::cout << "velocity: " << velocity << std::endl;
        // std::cout << "pid_pt: " << pid_pt << std::endl;
        // std::cout << std::endl;

        cv::line(img, cur_pt + cv::Point2f(-50, 0), cur_pt + cv::Point2f(50, 0), cv::Scalar(0, 0, 255), 1, 8);
        cv::line(img, cur_pt + cv::Point2f(0, -50), cur_pt + cv::Point2f(0, 50), cv::Scalar(0, 0, 255), 1, 8);
        cv::circle(img, pid_pt, 10, cv::Scalar(255, 0, 0), 2, 8);

        cv::imshow("img", img);
        char c = cv::waitKey(10);
        if (c == 'q')
            break;
    }

    return 0;
}